/**
 * @file IntegratedNavigation.hpp
 * @brief 误差状态卡尔曼滤波
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-06-13
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */

#ifndef _INTEGRATED_NAVIGATION_HPP
#define _INTEGRATED_NAVIGATION_HPP

#include <iostream>
#include <eigen3/Eigen/Dense>
#include "utils.hpp"

namespace Nav{

using Mat = Eigen::Matrix<double, 21, 21>;
using Vec = Eigen::Matrix<double, 21,  1>;

class IntegratedNavigation{
private:
    // δx_k+1 = Φ_k * δx_k + w_k
    Vec   state_err_; // 误差状态δx: δr δv δa bg ba sg sa
    Vec   delta_x_;     // 状态向量
    Eigen::Vector3d   delta_z_;     // 观测向量
    /* 状态更新 */
    Mat   PHI_;   // 状态转移矩阵Φ
    Mat   Q_;     // 过程噪声协方差矩阵Q
    Mat   q_;     // 连续时间系统噪声方差矩阵q
    Mat   q_last_;
    Mat   G_;
    Mat   G_last_;
    Mat   P_;     // 误差协方差矩阵P

    /* 观测更新 */
    // δzk = Hk * δxk + ek
    Mat   K_; // 卡尔曼增益矩阵K
    Mat   R_; // 观测噪声协方差矩阵R
    Eigen::Matrix<double, 3, 21>   H_; // 观测增益矩阵H

    Eigen::Vector3d l_;     // 天线杆臂向量
    Eigen::Matrix3d DR_;    

    Mechanization   mec_;
    ImuModel        imumodel_;
    StatePtr        state_;
    double          imudata_dt_;
    Data            data_last_;
    DataPtr         data_curr_;

public:
    IntegratedNavigation() = default;
    ~IntegratedNavigation() = default;
    
    // 初始化
    void initialize(const StatePtr& state, const DataPtr& data, ImuModel& imumodel){
        state_      = state;
        mec_        = Mechanization(state_, data->imudata);
        imumodel_   = imumodel;

        l_ << 0.136, -0.301, -0.184;
        delta_x_ <<  0, 0, 0,
                     0, 0, 0, 
                     0, 0, 0, 
                     0, 0, 0, 
                     0, 0, 0, 
                     0, 0, 0, 
                     0, 0, 0;

        data_last_  = *data;

        // 初始化P阵
        auto Zero_33       = Eigen::Matrix3d::Zero();
        Eigen::Matrix3d gb = Eigen::Vector3d(imumodel_.gyro_bias_std * imumodel_.gyro_bias_std, imumodel_.gyro_bias_std * imumodel_.gyro_bias_std, imumodel_.gyro_bias_std * imumodel_.gyro_bias_std).asDiagonal();
        Eigen::Matrix3d ab = Eigen::Vector3d(imumodel_.accel_bias_std * imumodel_.accel_bias_std, imumodel_.accel_bias_std * imumodel_.accel_bias_std, imumodel_.accel_bias_std * imumodel_.accel_bias_std).asDiagonal();
        Eigen::Matrix3d gs = Eigen::Vector3d(imumodel_.gyro_scale_factor_std * imumodel_.gyro_scale_factor_std, imumodel_.gyro_scale_factor_std * imumodel_.gyro_scale_factor_std, imumodel_.gyro_scale_factor_std * imumodel_.gyro_scale_factor_std).asDiagonal();
        Eigen::Matrix3d as = Eigen::Vector3d(imumodel_.accel_scale_factor_std * imumodel_.accel_scale_factor_std, imumodel_.accel_scale_factor_std * imumodel_.accel_scale_factor_std, imumodel_.accel_scale_factor_std * imumodel_.accel_scale_factor_std).asDiagonal();
        Eigen::Matrix3d posmat = (state_->pos_std).asDiagonal();
        posmat *= posmat;
        Eigen::Matrix3d velmat = (state_->vel_std).asDiagonal();
        velmat *= velmat;
        Eigen::Matrix3d attmat = (state_->att_std).asDiagonal();
        attmat *= attmat;
        
        P_ <<   posmat , Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33,
                Zero_33, velmat , Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, attmat , Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, gb     , Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, ab     , Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, gs     , Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, as     ;
    }

    void ImuErrCompensate(ImuData& imudata){
        // IMU误差补偿
        for(int i = 0; i < 3; i++){
            imudata.gyro_increm[i]  = (imudata.gyro_increm[i] - state_->gyro_bias[i] * imudata_dt_)
                                    / (1 + state_->gyro_scale_factor[i]);
            imudata.accel_increm[i] = (imudata.accel_increm[i] - state_->accel_bias[i] * imudata_dt_)
                                    / (1 + state_->accel_scale_factor[i]);
        }
    }

    void getPHImat(){
        Mat Fmat = Mat::Zero();

        auto I_33 = Eigen::Matrix3d::Identity();
        auto Zero_33 = Eigen::Matrix3d::Zero();

        auto pos = state_->pos;
        auto vel = state_->vel;
        auto att = state_->att;       

        double Rm, Rn;
        getRmRn(pos[LAT], Rm, Rn);
        auto g = getG(pos);
        Eigen::Matrix3d Frr, Fvr, Fvv, Fphir, Fphiv;
        Frr <<  -vel[D] / (Rm + pos[ALT]), 0, vel[N] / (Rm + pos[ALT]),
                vel[E] * tan(pos[LAT]) / (Rn + pos[ALT]), 
                -(vel[D] + vel[N] * tan(pos[LAT])) / (Rn + pos[ALT]), 
                vel[E] / (Rn + pos[ALT]),
                0, 0, 0;
        Fvr <<  -2 * vel[E] * OMEGA_E * cos(pos[LAT]) / (Rm + pos[ALT]) - vel[E] * vel[E] / (Rm + pos[ALT]) / (Rn + pos[ALT]) / cos(pos[LAT]) / cos(pos[LAT]),
                0,
                vel[N] * vel[D] / (Rm + pos[ALT]) / (Rm + pos[ALT]) - vel[E] * vel[E] * tan(pos[LAT]) / (Rn + pos[ALT]) / (Rn + pos[ALT]),
                2 * OMEGA_E * (vel[N] * cos(pos[LAT]) - vel[D] * sin(pos[LAT])) / (Rm + pos[ALT]) + vel[N] * vel[E] / (Rm + pos[ALT]) / (Rn + pos[ALT]) / cos(pos[LAT]) / cos(pos[LAT]),
                0,
                (vel[E] * vel[D] + vel[N] * vel[E] * tan(pos[LAT])) / (Rn + pos[ALT]) / (Rn + pos[ALT]),
                2 * OMEGA_E * vel[E] * sin(pos[LAT]) / (Rm + pos[ALT]),
                0,
                -vel[E] * vel[E] / (Rn + pos[ALT]) / (Rn + pos[ALT]) - vel[N] * vel[N] / (Rm + pos[ALT]) / (Rm + pos[ALT]) + 2 * g[D] / (sqrt(Rm * Rn) + pos[ALT]);
        Fvv <<  vel[D] / (Rm + pos[ALT]), -2 * (OMEGA_E * sin(pos[LAT]) + vel[E] * tan(pos[LAT]) / (Rn + pos[ALT])), vel[N] / (Rm + pos[ALT]),
                2 * OMEGA_E * sin(pos[LAT]) + vel[E] * tan(pos[LAT]) / (Rn + pos[ALT]), (vel[D] + vel[N] * tan(pos[LAT])) / (Rn + pos[ALT]), 2 * OMEGA_E * cos(pos[LAT]) + vel[E] / (Rn + pos[ALT]),
                -2 * vel[N] / (Rm + pos[ALT]), -2 * (OMEGA_E * cos(pos[LAT]) + vel[E] / (Rn + pos[ALT])), 0;
        Fphir <<    -OMEGA_E * sin(pos[LAT]) / (Rm + pos[ALT]), 0, vel[E] / (Rn + pos[ALT]) / (Rn + pos[ALT]),
                    0, 0, -vel[N] / (Rm + pos[ALT]) / (Rm + pos[ALT]),
                    -OMEGA_E * cos(pos[LAT]) / (Rm + pos[ALT]) - vel[E] / (Rm + pos[ALT]) / (Rn + pos[ALT]) / cos(pos[LAT]) / cos(pos[LAT]), 0, -vel[E] * tan(pos[LAT]) / (Rn + pos[ALT]) / (Rn + pos[ALT]);
        Fphiv <<    0, 1 / (Rn + pos[ALT]), 0,
                    -1 / (Rm + pos[ALT]), 0, 0,
                    0, -tan(pos[LAT]) / (Rn + pos[ALT]), 0;

        auto Cbn        = state_->q_nb.toRotationMatrix();
        auto omega_ib   = data_curr_->imudata.gyro_increm / imudata_dt_;
        auto f_b        = data_curr_->imudata.accel_increm / imudata_dt_;

        Eigen::Vector3d w_ie, w_en, w_in;
        w_ie <<  OMEGA_E * cos(pos[LAT]),
                 0,
                -OMEGA_E * sin(pos[LAT]);
        w_en <<  state_->vel[E] / (Rn + pos[ALT]),
                -state_->vel[N] / (Rm + pos[ALT]),
                -state_->vel[E] / (Rn + pos[ALT]) * tan(pos[LAT]);
        w_in = w_ie + w_en;

        Fmat << Frr, I_33, Zero_33, Zero_33, Zero_33,Zero_33, Zero_33,
                Fvr, Fvv, getAntiSymmetricMatrix(Cbn * f_b), Zero_33, Cbn, Zero_33, Cbn * f_b.asDiagonal(),
                Fphir, Fphiv, -getAntiSymmetricMatrix(w_in), -Cbn, Zero_33, -Cbn * omega_ib.asDiagonal(), Zero_33,
                Zero_33, Zero_33, Zero_33, -1 / imumodel_.gyro_bias_corr_time * I_33, Zero_33, Zero_33, Zero_33,
                Zero_33, Zero_33, Zero_33, Zero_33, -1 / imumodel_.accel_bias_corr_time * I_33, Zero_33, Zero_33,
                Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, -1 / imumodel_.gyro_scale_factor_corr_time * I_33, Zero_33,
                Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, -1 / imumodel_.accel_scale_factor_corr_time * I_33;

        // Φ = I + F(tk)∆t
        PHI_ = Mat::Identity() + Fmat * imudata_dt_;
    }

    void getQmat(){
        double item[] = {   0,
                            imumodel_.VRW * imumodel_.VRW,
                            imumodel_.ARW * imumodel_.ARW,
                            2 * imumodel_.gyro_bias_std * imumodel_.gyro_bias_std / imumodel_.gyro_bias_corr_time,
                            2 * imumodel_.accel_bias_std * imumodel_.accel_bias_std / imumodel_.accel_bias_corr_time,
                            2 * imumodel_.gyro_scale_factor_std * imumodel_.gyro_scale_factor_std / imumodel_.gyro_scale_factor_corr_time,
                            2 * imumodel_.accel_scale_factor_std * imumodel_.accel_scale_factor_std / imumodel_.accel_scale_factor_corr_time
                            };        
        Eigen::Matrix3d mat[7];
        for(int i = 0; i < 7; i++)
            mat[i] = Eigen::Vector3d(item[i], item[i], item[i]).asDiagonal();
        auto Zero_33 = Eigen::Matrix3d::Zero();
        q_ <<   mat[0], Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, mat[1], Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, mat[2], Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, mat[3], Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, mat[4], Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, mat[5], Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, mat[6];

        auto Cbn = state_->q_nb.toRotationMatrix();
        auto I_33 = Eigen::Matrix3d::Identity();
        
        G_ <<   Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, 
                Cbn,     Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, Cbn,     Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, I_33,    Zero_33, Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, I_33,    Zero_33, Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, I_33,    Zero_33, Zero_33, 
                Zero_33, Zero_33, Zero_33, Zero_33, Zero_33, I_33,    Zero_33;

        Q_ = 0.5 * (PHI_ * G_last_ * q_last_ * G_last_.transpose() * PHI_.transpose() + G_ * q_ * G_.transpose()) * imudata_dt_;

        q_last_ = q_;
        G_last_ = G_;
    }

    void KalmanPredict(){
        getPHImat();
        getQmat();

        // 计算先验估计
        delta_x_ = PHI_ * delta_x_;
        // 计算先验误差协方差矩阵
        P_ = PHI_ * P_ * PHI_.transpose() + Q_;
    }

    void KalmanCorrect(){
        auto Cbn     = state_->q_nb.toRotationMatrix();
        auto I_33    = Eigen::Matrix3d::Identity();
        auto Zero_33 = Eigen::Matrix3d::Zero();

        Eigen::Matrix<double, 3, 21> Hmat;
        Hmat << I_33, Zero_33, getAntiSymmetricMatrix(Cbn * l_), Zero_33, Zero_33, Zero_33, Zero_33;
        Eigen::Matrix3d Rmat = data_curr_->gnssdata.pos_std.asDiagonal();
        Rmat *= Rmat; // 标准差->方差

        // 计算δz_r
        // 位置误差观测向量可表示为INS推算的位置与GNSS位置观测之差
        double Rm, Rn;
        getRmRn(state_->pos[LAT], Rm, Rn);
        // 纬经高->北东地的转换矩阵
        DR_ <<  Rm + state_->pos[ALT], 0, 0,
                0, (Rn + state_->pos[ALT]) * cos(state_->pos[LAT]), 0,
                0 , 0, -1; 
        auto pos_ant = state_->pos + DR_.inverse() * Cbn * l_;
        auto delta_z_gnss = DR_ * (pos_ant - data_curr_->gnssdata.pos);

        // 计算卡尔曼增益
        // K = Pk * HT / (H * Pk * HT + R)
        Eigen::Matrix<double, 21, 3> Kmat = P_ * Hmat.transpose() * (Hmat * P_ * Hmat.transpose() + Rmat).inverse();
        // 更新后验估计
        delta_x_ = delta_x_ + Kmat * (delta_z_gnss - Hmat * delta_x_);
        // 更新误差协方差矩阵
        P_ = (Mat::Identity() - Kmat * Hmat) * P_ * (Mat::Identity() - Kmat * Hmat).transpose()
           + Kmat * Rmat * Kmat.transpose();
    }

    void KalmanFilter(const DataPtr& data_raw){
        data_curr_ = data_raw;
        imudata_dt_ = data_curr_->imudata.timestamp - data_last_.imudata.timestamp;

        // 机械编排和预测
        // 如果GNSS数据有效，同时gnss数据的时间戳在当前IMU数据和前一次IMU数据中间，则进行组合
        if(data_curr_->gnssdata.valid 
            && data_curr_->imudata.timestamp > data_curr_->gnssdata.timestamp
            && data_last_.imudata.timestamp < data_curr_->gnssdata.timestamp)
        {
            auto dt1 = data_curr_->imudata.timestamp - data_curr_->gnssdata.timestamp;
            auto dt2 = data_last_.imudata.timestamp - data_curr_->gnssdata.timestamp;
            // std::cout << dt1 << " " << dt2 << std::endl;
            /* 获取整秒状态，用于更新GNSS */
            // 如果GNSS数据在中间位置，进行内插
            ImuData imudata_mid = data_curr_->imudata;
            ImuData imudata_curr = data_curr_->imudata;

            auto ratio  = (data_curr_->gnssdata.timestamp - data_last_.imudata.timestamp)
                        / (data_curr_->imudata.timestamp - data_last_.imudata.timestamp);
            imudata_mid.timestamp = data_curr_->gnssdata.timestamp;
            imudata_mid.accel_increm *= ratio;
            imudata_mid.gyro_increm *= ratio;
            imudata_dt_ = imudata_mid.timestamp - data_last_.imudata.timestamp;

            // 计算整秒时刻机械编排的结果
            ImuErrCompensate(imudata_mid); // IMU误差补偿
            mec_.update(imudata_mid);
            
            data_curr_->imudata = imudata_mid;
            // 预测
            KalmanPredict();
            // 校准
            KalmanCorrect();

            /* 使用卡尔曼滤波得到的误差修正状态向量 */
            // 1. 位置修正
            // δr单位是米，要转换为rad，从(m, m, m)转换到(rad, rad, m)
            Eigen::Vector3d delta_r;
            delta_r << delta_x_[0 + N], delta_x_[0 + E], delta_x_[0 + D];
            delta_r = DR_.inverse() * delta_r;
            state_->pos -= delta_r;
            
            // 2. 速度修正
            for(int i = 0; i < 3; i++){
                state_->vel[i] -= delta_x_[3 + i];
            }
            
            // 3. 姿态修正
            Eigen::Quaterniond q_pn = Eigen::AngleAxisd(delta_x_[8], Eigen::Vector3d::UnitZ())
                                    * Eigen::AngleAxisd(delta_x_[7], Eigen::Vector3d::UnitY())
                                    * Eigen::AngleAxisd(delta_x_[6], Eigen::Vector3d::UnitX()) ;
            state_->q_nb = q_pn * state_->q_nb;
            state_->q_nb.normalize();
            DCM2Euler(state_->q_nb.toRotationMatrix(), state_->att);
            
            // 4. 零偏、比例因子修正
            for(int i = 0; i < 3; i++){
                state_->gyro_bias[i]            += delta_x_[ 9 + i];
                state_->accel_bias[i]           += delta_x_[12 + i];
                state_->gyro_scale_factor[i]    += delta_x_[15 + i];
                state_->accel_scale_factor[i]   += delta_x_[18 + i];
            }

            // 状态清零
            delta_x_ << 0, 0, 0,
                        0, 0, 0, 
                        0, 0, 0, 
                        0, 0, 0, 
                        0, 0, 0, 
                        0, 0, 0, 
                        0, 0, 0;

            /* 继续另一半的机械编排和预测 */
            data_curr_->imudata = imudata_curr;
            imudata_mid = data_curr_->imudata;
            imudata_mid.timestamp = data_curr_->imudata.timestamp;
            imudata_mid.accel_increm *= (1 - ratio);
            imudata_mid.gyro_increm *= (1 - ratio);
            imudata_dt_ = imudata_mid.timestamp - data_last_.gnssdata.timestamp;
            ImuErrCompensate(imudata_mid); // IMU误差补偿
            mec_.update(imudata_mid);
            KalmanPredict();

            data_curr_->gnssdata.valid = false;
        }
        else{ // 否则进行预测
            ImuErrCompensate(data_curr_->imudata); // IMU误差补偿
            mec_.update(data_curr_->imudata);
            KalmanPredict();
        }
        
        data_last_ = *data_curr_;
    }

};

};

#endif